#ifndef FOC_MOTOR_H
#define FOC_MOTOR_H

#include <Arduino.h>
#include <math.h>
#include "sensor.h"

struct PhaseVoltage
{
    double ua;
    double ub;
    double uc;
    PhaseVoltage(double ua, double ub, double uc)
        : ua(ua), ub(ub), uc(uc)
    {
    }
};

class FOC_Motor
{
private:
    int motor_pin_a;
    int motor_pin_b;
    int motor_pin_c;
    int motor_pwm_channel_a;
    int motor_pwm_channel_b;
    int motor_pwm_channel_c;
    unsigned long pole_pairs;
    double voltage_supply;
    double limit_voltage;
    double shaft_angle;
    unsigned long last_time;
    double target_velocity;
    double target_angle;
    bool openloop_flag;
    TaskHandle_t *openloop_handlep;
    bool closeloop_pos_flag;
    TaskHandle_t *closeloop_pos_handlep;
    AS5600 *sensorp;

    static void _openloop_rotate_wrapper(void *arg)
    {
        FOC_Motor *motorp = static_cast<FOC_Motor *>(arg);
        motorp->_openloop_rotate();
    }

    static void _closeloop_pos_wrapper(void *arg)
    {
        FOC_Motor *motorp = static_cast<FOC_Motor *>(arg);
        motorp->_closeloop_pos();
    }

    double _constrain(double data, double min, double max);
    double _normalize_angle(double angle);
    double _shaft2electrical(double angle);
    PhaseVoltage _foc(double uq, double ud, double electrical_angle);
    void _pwm(PhaseVoltage pv);
    void _openloop_rotate();
    void _closeloop_pos();

public:
    FOC_Motor(int motor_pin1, int motor_pin2, int motor_pin3, unsigned long pole_pairs, double voltage_supply, double limit_voltage, AS5600 *sensorp = NULL);
    void calibrate_sensor();
    void openloop_rotate_start(double velocity);
    void openloop_rotate_end();
    void closeloop_pos_start(double target_angle);
    void closeloop_pos_end();
};

#endif